'''
Title: Pupil: Eye Tracking Software
kalman_filter.py
Authors: Moritz Kassner
Date: July 12, 2011
Notes: Prototype tested with 2 XBox Live Webcams on Macbook Pro OSX 10.6.7 and Ubuntu 10.10 using OpenCV 2.2
'''

#Kalman Filter implematation
#The filter assumes linear dynamics of the system, 
#a linear relationship between mesurement and state,
#and (zero mean) gaussianitiy  in the error distributions


from numpy import *

def create_kalman_preset():
    
    dim = 4 # dimensionality of the filter

    #I assume these to be constant , they dont have to be...
    Ny = eye(dim/2)*0.00000001**2 #noise covarience of the Mesurement -> how much do I trust my sensors
    Nx =  eye(dim)*1e-5 #noise covarience of the Process -> how much do I trust my model
    B = zeros((dim/2,dim))# Tansformation of Mesurement to Estimated State  
    B[0,0] = 1
    B[1,1] = 1
    A = eye(dim) # The process transformation than governs the mesured system (this stands still)
    A[0,2] = 1
    A[1,3] = 1
    

    return Kalman_filter(Ny,Nx,B,A,dim)



class Kalman_filter():
    """Kalman_filter class
    its mostly used to save the state of the Filter from update to update"""
    def __init__(self,Ny,Nx,B,A,dim):
        self.Ny= matrix(Ny)
        self.Nx=  matrix(Nx)
        self.B =  matrix(B )
        self.A =  matrix(A)


        #will be overwritten before being read 
        self.K = None # Kalman Gain Matrix
        self.ye =None  #Estimation of the Observable given the old state 
        self.x = None # estimated state
        self.y = None  #observed value

        #initial guess
        self.xe = matrix(zeros((dim))).transpose()  #predicted new state 
        self.E =  matrix(eye(dim)*.5) # Error Matrix
         
        
    def update(self,mesurement):
        """preform one update on the kalman filter with the new mesurement"""
        self.ye = self.B * self.xe #Estimate the new Observable
        self.y = matrix(mesurement).transpose()#Measure a new value
        self.K = self.E*self.B.transpose()*linalg.pinv(self.B*self.E*self.B.transpose()+self.Ny) #Compute the Kalman Gain Matrix
        self.x = self.xe + self.K*(self.y-self.ye) #Estimate the new state #(y-ye) is the sensor innovation // K determins how much this is weighted
        
        #1-elm for non-zero elements??
        tmp = self.K*self.B
        tmp[nonzero(tmp)] = 1-tmp[nonzero(tmp)]
        self.E = tmp*self.E #Update the Error Matrix
        
        #how to do this better?
        self.x[2] =(self.y-self.ye)[0]
        self.x[3] =(self.y-self.ye)[1]
        
        
        self.xe = self.A*self.x #Predict the new State
        self.E = self.A*self.E*self.A.transpose()+self.Nx #Predict the new Error
        #print "K",self.K
        #print "E",self.E
        #print "state",self.x
        return self.x[0], self.x[1]
        
